/*--------------------------------------------------------------------------------------------------
Yatak Operation System Project Copyright 2012-2013
Author : Ilya Churaev
Started : 17 Jan 2013		Updated : 1 Jun 2013
File Name : pc.c		Purpose : Functions to programm a pc. (INTEL) outb, inb, outw, inw, real time clock access,...
--------------------------------------------------------------------------------------------------*/
#include <yatak/debug.h>
#include <yatak/port.h>
#include <string.h>
#include <screen.h>
#include <cpuid.h>

/* Required Declarations */
int do_intel(struct cpu_info *cpu);
int do_amd(struct cpu_info *cpu);
int do_cyrix(struct cpu_info *cpu);
void printregs(int eax, int ebx, int ecx, int edx);
char * getregs(int eax, int ebx, int ecx, int edx);

/* Intel Specific brand list */
char *Intel[] = {
	"Brand ID Not Supported.", 
	"Intel(R) Celeron(R) processor", 
	"Intel(R) Pentium(R) III processor", 
	"Intel(R) Pentium(R) III Xeon(R) processor", 
	"Intel(R) Pentium(R) III processor", 
	"Reserved", 
	"Mobile Intel(R) Pentium(R) III processor-M", 
	"Mobile Intel(R) Celeron(R) processor", 
	"Intel(R) Pentium(R) 4 processor", 
	"Intel(R) Pentium(R) 4 processor", 
	"Intel(R) Celeron(R) processor", 
	"Intel(R) Xeon(R) Processor", 
	"Intel(R) Xeon(R) processor MP", 
	"Reserved", 
	"Mobile Intel(R) Pentium(R) 4 processor-M", 
	"Mobile Intel(R) Pentium(R) Celeron(R) processor", 
	"Reserved", 
	"Mobile Genuine Intel(R) processor", 
	"Intel(R) Celeron(R) M processor", 
	"Mobile Intel(R) Celeron(R) processor", 
	"Intel(R) Celeron(R) processor", 
	"Mobile Geniune Intel(R) processor", 
	"Intel(R) Pentium(R) M processor", 
	"Mobile Intel(R) Celeron(R) processor"
};

/* This table is for those brand strings that have two values depending on the processor signature. It should have the same number of entries as the above table. */
char *Intel_Other[] = {
	"Reserved", 
	"Reserved", 
	"Reserved", 
	"Intel(R) Celeron(R) processor", 
	"Reserved", 
	"Reserved", 
	"Reserved", 
	"Reserved", 
	"Reserved", 
	"Reserved", 
	"Reserved", 
	"Intel(R) Xeon(R) processor MP", 
	"Reserved", 
	"Reserved", 
	"Intel(R) Xeon(R) processor", 
	"Reserved", 
	"Reserved", 
	"Reserved", 
	"Reserved", 
	"Reserved", 
	"Reserved", 
	"Reserved", 
	"Reserved", 
	"Reserved"
};

/* Simply call this function detect_cpu(); */
int detect_cpu(struct cpu_info *cpu)
{
	unsigned long ebx, unused;
	cpuid(0, unused, ebx, unused, unused);
	dprintf ("[detect_cpu] ebx = 0x%x.",ebx);
	cpu->vendor_id = ebx;
	switch(ebx)
	{
	case 0x756e6547: /* Intel Magic Code */
		dprintf (" It is Intel magic code...\n");
		do_intel(cpu);
		break;
	case 0x68747541: /* AMD Magic Code */
		dprintf (" It is AMD magic code...\n");
		do_amd(cpu);
		break;
	case 0x69727943: /* Cyrix Magic Code */
		dprintf (" It is Cyrix magic code...\n");
		do_cyrix(cpu);
		break;
	default:
		dprintf (" It is Unknown magic code...\n");
		kprintf("Unknown x86 CPU Detected\n");
		break;
	}
	MessageFoS ("CPU detecting...\t\t\t\t\t\t",true);
	return 0;
}

/* Intel-specific information */
int do_intel(struct cpu_info *cpu) 
{
	kprintf("Intel Specific Features:\n");
	unsigned long eax, ebx, ecx, edx, max_eax, signature, unused;
	int model, family, type, brand, stepping, reserved;
	int extended_family = -1;
	cpuid(1, eax, ebx, unused, unused);
	model = (eax >> 4) & 0xf;
	family = (eax >> 8) & 0xf;
	type = (eax >> 12) & 0x3;
	brand = ebx & 0xff;
	stepping = eax & 0xf;
	reserved = eax >> 14;
	signature = eax;
	kprintf("Type %d - ", type);
	cpu->type = type;
	switch(type)
	{
	case 0:
		kprintf("Original OEM");
		strcpy (cpu->type_str, "Original OEM");
		break;
	case 1:
		kprintf("Overdrive");
		strcpy (cpu->type_str, "Overdrive");
		break;
	case 2:
		kprintf("Dual-capable");
		strcpy (cpu->type_str, "Dual-capable");
		break;
	case 3:
		kprintf("Reserved");
		strcpy (cpu->type_str, "Reserved");
		break;
	}
	kprintf("\n");
	kprintf("Family %d - ", family);
	cpu->family = family;
	switch(family) 
	{
	case 3:
		kprintf("i386");
		strcpy (cpu->family_str, "i386");
		break;
	case 4:
		kprintf("i486");
		strcpy (cpu->family_str, "i486");
		break;
	case 5:
		kprintf("Pentium");
		strcpy (cpu->family_str, "Pentium");
		break;
	case 6:
		kprintf("Pentium Pro");
		strcpy (cpu->family_str, "Pentium Pro");
		break;
	case 15:
		kprintf("Pentium 4");
		strcpy (cpu->family_str, "Pentium 4");
	}
	kprintf("\n");
	if(family == 15) 
	{
		extended_family = (eax >> 20) & 0xff;
		kprintf("Extended family %d\n", extended_family);
	}
	kprintf("Model %d - ", model);
	cpu->model = model;
	switch(family)
	{
	case 3:
		strcpy (cpu->model_str, "\0");
		break;
	case 4:
		switch(model) 
		{
		case 0:
		case 1:
			kprintf("DX");
			strcpy (cpu->model_str, "DX");
			break;
		case 2:
			kprintf("SX");
			strcpy (cpu->model_str, "SX");
			break;
		case 3:
			kprintf("487/DX2");
			strcpy (cpu->model_str, "487/DX2");
			break;
		case 4:
			kprintf("SL");
			strcpy (cpu->model_str, "SL");
			break;
		case 5:
			kprintf("SX2");
			strcpy (cpu->model_str, "SX2");
			break;
		case 7:
			kprintf("Write-back enhanced DX2");
			strcpy (cpu->model_str, "Write-back enhanced DX2");
			break;
		case 8:
			kprintf("DX4");
			strcpy (cpu->model_str, "DX4");
			break;
		}
		break;
	case 5:
		switch(model) 
		{
		case 1:
			kprintf("60/66");
			strcpy (cpu->model_str, "60/66");
			break;
		case 2:
			kprintf("75-200");
			strcpy (cpu->model_str, "75-200");
			break;
		case 3:
			kprintf("for 486 system");
			strcpy (cpu->model_str, "for 486 system");
			break;
		case 4:
			kprintf("MMX");
			strcpy (cpu->model_str, "MMX");
			break;
		}
		break;
	case 6:
		switch(model)
		{
		case 1:
			kprintf("Pentium Pro");
			strcpy (cpu->model_str, "Pentium Pro");
			break;
		case 3:
			kprintf("Pentium II Model 3");
			strcpy (cpu->model_str, "Pentium II Model 3");
			break;
		case 5:
			kprintf("Pentium II Model 5/Xeon/Celeron");
			strcpy (cpu->model_str, "Pentium II Model 5/Xeon/Celeron");
			break;
		case 6:
			kprintf("Celeron");
			strcpy (cpu->model_str, "Celeron");
			break;
		case 7:
			kprintf("Pentium III/Pentium III Xeon - external L2 cache");
			strcpy (cpu->model_str, "Pentium III/Pentium III Xeon - external L2 cache");
			break;
		case 8:
			kprintf("Pentium III/Pentium III Xeon - internal L2 cache");
			strcpy (cpu->model_str, "Pentium III/Pentium III Xeon - internal L2 cache");
			break;
		}
		break;
	case 15:
		break;
	}
	kprintf("\n");
	cpuid(0x80000000, max_eax, unused, unused, unused);
	/* Quok said: If the max extended eax value is high enough to support the processor
	 brand string (values 0x80000002 to 0x80000004), then we'll use that information to
	 return the brand information. Otherwise, we'll refer back to the brand tables above
	 for backwards compatibility with older processors. According to the Sept. 2006 Intel
	 Arch Software Developer's Guide, if extended eax values are supported, then all 3
	 values for the processor brand string are supported, but we'll test just to make sure
	 and be safe. */
	if(max_eax >= 0x80000004) 
	{
		char str1 [17] = {'\0'}, str2 [17] = {'\0'}, str3 [17] = {'\0'};
		kprintf("Brand: ");
		if(max_eax >= 0x80000002)
		{
			cpuid(0x80000002, eax, ebx, ecx, edx);
			printregs(eax, ebx, ecx, edx);
			strcpy (str1, getregs (eax, ebx, ecx, edx));
		}
		if(max_eax >= 0x80000003)
		{
			cpuid(0x80000003, eax, ebx, ecx, edx);
			printregs(eax, ebx, ecx, edx);
			strcpy (str2, getregs (eax, ebx, ecx, edx));
		}
		if(max_eax >= 0x80000004)
		{
			cpuid(0x80000004, eax, ebx, ecx, edx);
			printregs(eax, ebx, ecx, edx);
			strcpy (str3, getregs (eax, ebx, ecx, edx));
		}
		sprintf (cpu->brand,"%s%s%s",str1,str2,str3);
		kprintf("\n");
	}
	else if(brand > 0)
	{
		kprintf("Brand %d - ", brand);
		if(brand < 0x18)
		{
			if(signature == 0x000006B1 || signature == 0x00000F13)
			{
				kprintf("%s\n", Intel_Other[brand]);
				sprintf (cpu->brand, "%d - %s",Intel_Other[brand]);
			}
			else
			{
				kprintf("%s\n", Intel[brand]);
				sprintf (cpu->brand, "%d - %s",Intel[brand]);
			}
		}
		else
		{
			kprintf("Reserved\n");
			strcpy (cpu->brand,"Reserved");
		}
	}
	kprintf("Stepping: %d Reserved: %d\n", stepping, reserved);
	cpu->stepping = stepping;
	cpu->reserved = reserved;
	return 0;
}

/* AMD-specific information */
int do_amd(struct cpu_info *cpu) 
{
	kprintf("AMD Specific Features:\n");
	unsigned long extended, eax, ebx, ecx, edx, unused;
	int family, model, stepping, reserved;
	cpuid(1, eax, unused, unused, unused);
	model = (eax >> 4) & 0xf;
	family = (eax >> 8) & 0xf;
	stepping = eax & 0xf;
	reserved = eax >> 12;
	kprintf("Family: %d Model: %d [", family, model);
	cpu->family = family;
	cpu->model = model;
	switch(family)
	{
	case 4:
		kprintf("486 Model %d", model);
		sprintf(cpu->model_str,"486 Model %d", model);
		break;
	case 5:
		switch(model)
		{
		case 0:
		case 1:
		case 2:
		case 3:
		case 6:
		case 7:
			kprintf("K6 Model %d", model);
			sprintf(cpu->model_str,"K6 Model %d", model);
			break;
		case 8:
			kprintf("K6-2 Model 8");
			sprintf(cpu->model_str,"K6-2 Model 8");
			break;
		case 9:
			kprintf("K6-III Model 9");
			sprintf(cpu->model_str,"K6-III Model 9");
			break;
		default:
			kprintf("K5/K6 Model %d", model);
			sprintf(cpu->model_str,"K5/K6 Model %d", model);
			break;
		}
		break;
	case 6:
		switch(model)
		{
		case 1:
		case 2:
		case 4:
			kprintf("Athlon Model %d", model);
			sprintf(cpu->model_str,"Athlon Model %d", model);
			break;
		case 3:
			kprintf("Duron Model 3");
			sprintf(cpu->model_str,"Duron Model 3");
			break;
		case 6:
			kprintf("Athlon MP/Mobile Athlon Model 6");
			sprintf(cpu->model_str,"Athlon MP/Mobile Athlon Model 6");
			break;
		case 7:
			kprintf("Mobile Duron Model 7");
			sprintf(cpu->model_str,"Mobile Duron Model 7");
			break;
		default:
			kprintf("Duron/Athlon Model %d", model);
			sprintf(cpu->model_str,"Duron/Athlon Model %d", model);
			break;
		}
		break;
	}
	kprintf("]\n");
	cpuid(0x80000000, extended, unused, unused, unused);
	if(extended == 0)
	{
		return 0;
	}
	if(extended >= 0x80000002)
	{
		unsigned int j;
		kprintf("Detected Processor Name: ");
		for(j = 0x80000002; j <= 0x80000004; j++)
		{
			cpuid(j, eax, ebx, ecx, edx);
			printregs(eax, ebx, ecx, edx);
		}
		char str1 [17] = {'\0'}, str2 [17] = {'\0'}, str3 [17] = {'\0'};
		cpuid(0x80000002, eax, ebx, ecx, edx);
		strcpy (str1, getregs(eax, ebx, ecx, edx));
		cpuid(0x80000003, eax, ebx, ecx, edx);
		strcpy (str2, getregs(eax, ebx, ecx, edx));
		cpuid(0x80000004, eax, ebx, ecx, edx);
		strcpy (str3, getregs(eax, ebx, ecx, edx));
		sprintf (cpu->brand,"%s%s%s",str1,str2,str3);
		kprintf("\n");
	}
	if(extended >= 0x80000007)
	{
		cpuid(0x80000007, unused, unused, unused, edx);
		if(edx & 1)
		{
			kprintf("Temperature Sensing Diode Detected!\n");
		}
	}
	kprintf("Stepping: %d Reserved: %d\n", stepping, reserved);
	cpu->stepping = stepping;
	cpu->reserved = reserved;
	return 0;
}

int do_cyrix(struct cpu_info *cpu)
{
	kprintf("Cyrix-specific functions:\n");
	unsigned long eax, unused;
	int model, family, stepping, reserved;
	cpuid(1, eax, unused, unused, unused);
	model = (eax >> 4) & 0xf;
	family = (eax >> 8) & 0xf;
	stepping = eax & 0xf;
	reserved = eax >> 12;
	kprintf("Family: %d Model: %d [", family, model);
	cpu->model = model;
	cpu->family = family;
	switch(family)
	{
	case 4:
		switch(model)
		{
		case 4:
			kprintf("MediaGX");
			strcpy (cpu->model_str,"MediaGX");
			break;
		}
		break;
	case 5:
		switch(model)
		{
		case 2:
			kprintf("6x86");
			strcpy (cpu->model_str,"6x86");
			break;
		case 4:
			kprintf("BXm");
			strcpy (cpu->model_str,"BXm");
			break;
		}
		break;
	case 6:
		switch(model)
		{
		case 0:
			kprintf("6x86/MX");
			strcpy (cpu->model_str,"6x86/MX");
			break;
		}
		break;
	}
	kprintf("Stepping %d ", stepping);
	kprintf("Reserved %d\n", reserved);
	cpu->stepping = stepping;
	cpu->reserved = reserved;
	return 0;
}

/* Print Registers */
void printregs(int eax, int ebx, int ecx, int edx)
{
	int j;
	char string[17];
	string[16] = '\0';
	for(j = 0; j < 4; j++)
	{
		string[j] = eax >> (8 * j);
		string[j + 4] = ebx >> (8 * j);
		string[j + 8] = ecx >> (8 * j);
		string[j + 12] = edx >> (8 * j);
	}
	kprintf("%s", string);
}

char * getregs(int eax, int ebx, int ecx, int edx)
{
	int j;
	char string[17];
	string[16] = '\0';
	for(j = 0; j < 4; j++)
	{
		string[j] = eax >> (8 * j);
		string[j + 4] = ebx >> (8 * j);
		string[j + 8] = ecx >> (8 * j);
		string[j + 12] = edx >> (8 * j);
	}
	return string;
}

unsigned char inportb (unsigned short _port)
{
  unsigned char rv;
  __asm__ __volatile__ ("inb %1, %0"
	  : "=a" (rv)
	  : "d" (_port));
  return rv;
}

unsigned short inportw (unsigned short _port)
{
	unsigned short rv;
  __asm__ __volatile__ ("inw %1, %0"
	  : "=a" (rv)
	  : "d" (_port));
  return rv;
}

void insw(unsigned short port, unsigned short *addr, unsigned int count)
{
 __asm__ __volatile__ ("cld ; rep ; insw" \
: "=D" (addr), "=c" (count) : "d" (port),"0" (addr),"1" (count));
}

void insl(unsigned short port, unsigned int *addr, unsigned int count)
{
 __asm__ __volatile__ ("cld ; rep ; insl" \
: "=D" (addr), "=c" (count) : "d" (port),"0" (addr),"1" (count));
}

void outsw(unsigned short port, const unsigned short * addr, unsigned int count)
{
  __asm__ __volatile__ ("cld ; rep ; outsw" \
: "=S" (addr), "=c" (count) : "d" (port),"0" (addr),"1" (count));
}

void outsl(unsigned short port, const unsigned int * addr, unsigned int count)
{
  __asm__ __volatile__ ("cld ; rep ; outsl" \
: "=S" (addr), "=c" (count) : "d" (port),"0" (addr),"1" (count));
}


unsigned long inportl (unsigned short _port)
{
  unsigned long rv;
  __asm__ __volatile__ ("inl %1, %0"
	  : "=a" (rv)
	  : "d" (_port));
  return rv;
}

void outportb (unsigned short _port, unsigned char _data)
{
  __asm__ __volatile__ ("outb %1, %0"
	  :
	  : "d" (_port),
		"a" (_data));
}

void outportw (unsigned short _port, unsigned short _data)
{
  __asm__ __volatile__ ("outw %1, %0"
	  :
	  : "d" (_port),
		"a" (_data));
}

void outportl (unsigned short _port, unsigned long _data)
{
  __asm__ __volatile__ ("outl %1, %0"
	  :
	  : "d" (_port),
		"a" (_data));
}

void memory_dump(int addr, int bytes)
{
  char str[255]={0};
  char str2[255]={0};
  int i = 0;
  unsigned char* pointer;

  pointer = (unsigned char*) addr;

  kprintf("Memory dump of area 0x%00000008x - 0x%00000008x\n",addr,addr+bytes);

  while (bytes)
  {
	sprintf(str, "%02X ",*pointer);
	strcat(str2, str);
	pointer++;
	bytes--;

	i++;
	if (i > 24)
	{
	  i = 0;
	  kprintf(str2);
	  memset(str2,0,255);
	}
  }
  kprintf("\n");
}

void mem_dump_char(int addr, int bytes)
{
  char a_str[255]={0};
  char a_str2[255]={0};
  int i = 0;
  unsigned char* pointer;

  pointer = (unsigned char*) addr;

  while (bytes)
  {
	sprintf(a_str, "%c",*pointer);
	strcat(a_str2, a_str);
	pointer++;
	bytes--;

	i++;
	if (i > 30)
	{
	  i = 0;
	  kprintf(a_str2);
	  memset(a_str2,0,255);
	}
	else if (bytes==0)
	{
	  i = 0;
	  kprintf(a_str2);
	  memset(a_str2,0,255);
	}
  }
  kprintf("\n");
}

void mem_dump(int addr, int bytes)
{
  char str[255]={0};
  char str2[255]={0};
  char a_str[255]={0};
  char a_str2[255]={0};
  int i = 0;
  unsigned char* pointer;

  sprintf(a_str2, "	");

  pointer = (unsigned char*) addr;

  while (bytes)
  {
	sprintf(str, "%02X ",*pointer);

	sprintf(a_str, "%c",*pointer);
	strcat(str2, str);
	strcat(a_str2, a_str);
	pointer++;
	bytes--;

	i++;
	if (i > 14)
	{
	  i = 0;
	  strcat(str2, a_str2);
	  kprintf(str2);
	  memset(str2,0,255);
	  sprintf(a_str2,"	");
	}
	else if (bytes==0)
	{
	  i = 0;
	  strcat(str2, a_str2);
	  kprintf(str2);
	  memset(str2,0,255);
	  sprintf(a_str2,"	");
	}
  }
  kprintf("\n");
}

unsigned get_eflags()
{
	unsigned eflags;
	asm volatile("		\
		jmp	1f;			\
	1:	jmp	1f;			\
	1:	jmp	1f;			\
	1:	pushf;			\
		jmp	1f;			\
	1:	jmp	1f;			\
	1:	jmp	1f;			\
	1:	popl %0;" : "=r" (eflags));
	return eflags;
}

void set_eflags(unsigned eflags)
{
	asm volatile("		\
		pushl %0;		\
		jmp	1f;			\
	1:	jmp	1f;			\
	1:	jmp	1f;			\
	1:	popf;			\
		jmp	1f;			\
	1:	jmp	1f;			\
	1:	jmp	1f;			\
	1:	" : : "r" (eflags));
}


/*void cpuid(struct cpu_info *out_id)
{
	int orig_eflags = get_eflags();

	memset(out_id, 0, sizeof(*out_id));

	// Check for a dumb old 386 by trying to toggle the AC flag
	set_eflags(orig_eflags ^ EFL_AC);

	if ((get_eflags() ^ orig_eflags) & EFL_AC)
	{
		// It's a 486 or better.  Now try toggling the ID flag. 
		set_eflags(orig_eflags ^ EFL_ID);
		if ((get_eflags() ^ orig_eflags) & EFL_ID)
		{
			int highest_val;

			//
			// CPUID is supported, so use it.
			// First get the vendor ID string.
			//
			asm volatile("			\
				cpuid;				\
			" : "=a" (highest_val),
				"=b" (*((int*)(out_id->vendor_id+0))),
				"=d" (*((int*)(out_id->vendor_id+4))),
				"=c" (*((int*)(out_id->vendor_id+8)))
			  : "a" (0));

			// Now the feature information.
			if (highest_val >= 1)
			{
				asm volatile("		\
					cpuid;			\
				" : "=a" (*((int*)out_id)),
					"=d" (out_id->feature_flags)
				  : "a" (1)
				  : "ebx", "ecx");
			}

			// Cache and TLB information.
//			if (highest_val >= 2)
//				get_cache_config(out_id);
		}
		else
		{
			// No CPUID support - it's an older 486.
			out_id->family = CPU_FAMILY_486;

			// XXX detect FPU
		}
	}
	else
	{
		out_id->family = CPU_FAMILY_386;

		// XXX detect FPU
	}
	set_eflags(orig_eflags);
}*/
/*
void dump_cpuinfo(struct cpu_info *cpu)
{
   char scpu[50];
   char sfpu[50];
   unsigned int *pidata;
   struct cpuinfo_x86 *c;

   c = (unsigned int*)0x80600;

   kprintf("Processor   : 1\n");
   kprintf("Vendor ID   : %s\n",c->x86_vendor_id[0] ? c->x86_vendor_id : "unknown");

   if (cpu->family == CPU_FAMILY_386)
	 sprintf(scpu,"80386-Class CPU");
   if (cpu->family == CPU_FAMILY_486)
	 sprintf(scpu,"80486-Class CPU");
   if (cpu->family == CPU_FAMILY_PENTIUM)
	 sprintf(scpu,"Pentium-Class(\"586\")");
   if (cpu->family == CPU_FAMILY_PENTIUM_PRO)
	 sprintf(scpu,"Pentium-Pro-Class(\"686\")");

   if (cpu->family > CPU_FAMILY_486)
   {
	 if (cpu->feature_flags & CPUF_ON_CHIP_FPU)
	 {
	   sprintf(sfpu,"Build in floating-point unit.");
	   //register_hardware(DEVICE_IDENT_CPU, "Floating Point Unit", 0);
	 }
	 else
	   sprintf(sfpu,"No Build in floating-point unit.");

	 kprintf("%s\t%s\n",scpu,sfpu);

   }
   else kprintf("%s\n",scpu);

   //register_hardware(DEVICE_IDENT_CPU, scpu, 0);
}

void get_time2(struct time *t)
{
	  t->sec = CMOS_READ(RTC_SECONDS);
	  t->min = CMOS_READ(RTC_MINUTES);
	  t->hour = CMOS_READ(RTC_HOURS);
	  t->day = CMOS_READ(RTC_DAY_OF_MONTH);
	  t->mon = CMOS_READ(RTC_MONTH);
	  t->year = CMOS_READ(RTC_YEAR);
	  BCD_TO_BIN(t->sec);
	  BCD_TO_BIN(t->min);
	  BCD_TO_BIN(t->hour);
	  BCD_TO_BIN(t->day);
	  BCD_TO_BIN(t->mon);
	  BCD_TO_BIN(t->year);
}

unsigned int hex2dec(unsigned char *str)
{
  unsigned char len;
  unsigned short count;
  unsigned char fail;

  unsigned int mult;

  unsigned char ch;
  unsigned int dec;

  len = strlen(str)-1;
  count = len;
  fail = 0;

  mult = 1;
  dec = 0;

  do
  {
	ch = str[count];

	if ((ch >= '0') && (ch <= '9'))
	  dec += mult * (ch - 0x30);
	else if ((ch >= 'a') && (ch <= 'f'))
	  dec += mult * (ch - 87);
	else if ((ch >= 'A') && (ch <= 'F'))
	  dec += mult * (ch - 55);
	else fail = 1;

	if (count>0)
	  count--;
	else fail = 1;
	mult = mult * 16;
  } while  ((count >= 0) && (!fail));

  return dec;

}*/
